using System;
using System.Collections.Generic;
using System.Text;
using MyMath;

namespace Physics
{
    public class ConstrainingMesh:ConstrainingObject
    {
        CollisionMesh mesh;
        public ConstrainingMesh(string meshName)
        {
            mesh = CollisionMesh.FromFile(meshName, false);
        }
        public ConstrainingMesh(CollisionMesh mesh)
        {
            this.mesh = mesh;
        }
        public override bool IntersectSphere(MyVector position, MyVector velocity, CollisionSphere sphere, out MyVector intersectionPoint, out MyVector intersectionNormal, out MyVector translateBack)
        {
            intersectionPoint = new MyVector();
            intersectionNormal = new MyVector();


            int nPoints = 0;
            //bool status = false;
            MyVector iPoint=new MyVector(),iNormal=new MyVector(),translation=new MyVector();
            MyVector n1, n2, n3;
            MyVector point;
            float dist, d1, d2, d3;

            // check for face collision
            for (int i = 0; i < mesh.m_faces.Length; i++)
            {

                point = position - mesh.m_faces[i].n * sphere.Radius;


                if ((point - mesh.m_vertices[mesh.m_faces[i].v1]) * mesh.m_faces[i].n > 0)
                {
                    //the line doesn't cross the triangle
                    continue;
                }
                dist = mesh.m_faces[i].n * (point - mesh.m_vertices[mesh.m_faces[i].v1]);


                if (velocity.LengthSq > 0)
                {
                    intersectionPoint = point- velocity * (float)(dist / (velocity * mesh.m_faces[i].n));
                }
                else
                {
                    intersectionPoint = point + (float)dist * mesh.m_faces[i].n;
                }
                translateBack = intersectionPoint - point;

                //3 planes around triangle
                //plane1
                n1 = ((mesh.m_vertices[mesh.m_faces[i].v2] - mesh.m_vertices[mesh.m_faces[i].v1]) ^ mesh.m_faces[i].n).Normalize();
                d1 = n1 * (mesh.m_vertices[mesh.m_faces[i].v1]);

                //plane2
                n2 = ((mesh.m_vertices[mesh.m_faces[i].v3] - mesh.m_vertices[mesh.m_faces[i].v2]) ^ mesh.m_faces[i].n).Normalize();
                d2 = n2 * (mesh.m_vertices[mesh.m_faces[i].v2]);

                //plane3
                n3 = ((mesh.m_vertices[mesh.m_faces[i].v1] - mesh.m_vertices[mesh.m_faces[i].v3]) ^ mesh.m_faces[i].n).Normalize();
                d3 = n3 * (mesh.m_vertices[mesh.m_faces[i].v3]);

                float x1 = n1 * iPoint - d1;
                float x2 = n2 * iPoint - d2;
                float x3 = n3 * iPoint - d3;
                if (x1 <= 0 && x2 <= 0 && x3 <= 0)
                {
                    nPoints++;
                    iPoint.Add(intersectionPoint);
                    iNormal.Add(mesh.m_faces[i].n);
                    translation += translateBack;                    
                }

            }
           
            //check for edge collision

            foreach (CEdge e in mesh.m_edges)
            {

                MyVector DV, DVP;
                DV = mesh.m_vertices[e.v2] - mesh.m_vertices[e.v1];
                DVP = mesh.m_vertices[e.v1] - position;

                MyVector N = DVP - (DV * (DVP * DV));
                if (N.LengthSq < sphere.Radius * sphere.Radius)
                {
                    MyVector tmp = position + N * (sphere.Radius / N.Length);

                    if (velocity.LengthSq > 0)
                    {
                        intersectionPoint = position + tmp - velocity * (float)((tmp - N).Length / (velocity * N));

                        intersectionNormal = -velocity;
                        intersectionNormal.Normalize();
                    }
                    else
                    {
                        intersectionPoint = position + N;
                        intersectionNormal = -N;
                        intersectionNormal.Normalize();
                    }
                    translateBack = intersectionPoint - tmp;
                    if ((intersectionPoint - mesh.m_vertices[e.v1]) * DV >= 0 && (intersectionPoint - mesh.m_vertices[e.v2]) * DV <= 0)
                    {
                        nPoints++;
                        iPoint.Add(intersectionPoint);
                        iNormal.Add(intersectionNormal);
                        translation += translateBack;
                    }
                }
                
            }
            //no edge collision
            //check for vertex collision
            for (int i = 0; i < mesh.m_vertices.Length; i++)
            {
                MyVector DV;
                DV = mesh.m_vertices[i] - position;

                if (DV.LengthSq < sphere.Radius * sphere.Radius)
                {
                    MyVector tmp = position + DV * (sphere.Radius / DV.Length);

                    if (velocity.LengthSq > 0)
                    {
                        intersectionPoint = mesh.m_vertices[i];

                        intersectionNormal = -velocity;
                        intersectionNormal.Normalize();

                        translateBack = intersectionNormal * (float)(Math.Sqrt(sphere.Radius * sphere.Radius - DV.LengthSq));
                    }
                    else
                    {
                        intersectionPoint = mesh.m_vertices[i];

                        intersectionNormal = -DV;
                        intersectionNormal.Normalize();

                        translateBack = intersectionPoint - tmp;
                    }
                    
                    
                    nPoints++;
                    iPoint.Add(intersectionPoint);
                    iNormal.Add(intersectionNormal);
                    translation += translateBack;
                    
                }
            }
            if (nPoints > 1)
            {
                iPoint.Divide(nPoints);
                iNormal.Normalize();
                translation.Divide(nPoints);
            }

            intersectionPoint = iPoint;
            intersectionNormal = iNormal;
            translateBack = translation;


            return (nPoints > 0);
        }

        public override bool IntersectPoint(MyVector position, MyVector velocity, out MyVector intersectionPoint, out MyVector intersectionNormal, out MyVector translateBack)
        {
            intersectionPoint = new MyVector();
            intersectionNormal = new MyVector();


            int nPoints = 0;
            //bool status = false;
            MyVector iPoint = new MyVector(), iNormal = new MyVector(), translation = new MyVector();
            MyVector n1, n2, n3;
            float dist, d1, d2, d3;

            // check for face collision
            for (int i = 0; i < mesh.m_faces.Length; i++)
            {



                if ((position - mesh.m_vertices[mesh.m_faces[i].v1]) * mesh.m_faces[i].n > 0)
                {
                    //the line doesn't cross the triangle
                    continue;
                }
                dist = mesh.m_faces[i].n * (position - mesh.m_vertices[mesh.m_faces[i].v1]);


                if (velocity.LengthSq > 0)
                {
                    float time=dist / (velocity * mesh.m_faces[i].n);
                    if (time > 0.01f)
                        intersectionPoint = position - velocity * time;
                    else
                        intersectionPoint = position - (float)dist * mesh.m_faces[i].n;
                }
                else
                {
                    intersectionPoint = position - (float)dist * mesh.m_faces[i].n;
                }
                translateBack = intersectionPoint - position;

                //3 planes around triangle
                //plane1
                n1 = ((mesh.m_vertices[mesh.m_faces[i].v2] - mesh.m_vertices[mesh.m_faces[i].v1]) ^ mesh.m_faces[i].n).Normalize();
                d1 = n1 * (mesh.m_vertices[mesh.m_faces[i].v1]);

                //plane2
                n2 = ((mesh.m_vertices[mesh.m_faces[i].v3] - mesh.m_vertices[mesh.m_faces[i].v2]) ^ mesh.m_faces[i].n).Normalize();
                d2 = n2 * (mesh.m_vertices[mesh.m_faces[i].v2]);

                //plane3
                n3 = ((mesh.m_vertices[mesh.m_faces[i].v1] - mesh.m_vertices[mesh.m_faces[i].v3]) ^ mesh.m_faces[i].n).Normalize();
                d3 = n3 * (mesh.m_vertices[mesh.m_faces[i].v3]);

                float x1 = n1 * iPoint - d1;
                float x2 = n2 * iPoint - d2;
                float x3 = n3 * iPoint - d3;
                if (x1 <= 0 && x2 <= 0 && x3 <= 0)
                {
                    nPoints++;
                    iPoint.Add(intersectionPoint);
                    iNormal.Add(mesh.m_faces[i].n);
                    translation += translateBack;
                }

            }

        
            if (nPoints > 1)
            {
                iPoint.Divide(nPoints);
                iNormal.Normalize();
                translation.Divide(nPoints);
            }

            intersectionPoint = iPoint;
            intersectionNormal = iNormal;
            translateBack = translation;


            return (nPoints > 0);
        }
    }
}
